﻿//Copyright (C) 2012 Leif Foged

//Permission is hereby granted, free of charge, to any person obtaining a copy of
//this software and associated documentation files (the "Software"), to deal in
//the Software without restriction, including without limitation the rights to
//use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
//of the Software, and to permit persons to whom the Software is furnished to do
//so, subject to the following conditions:

//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.

//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.

using System.Collections.Generic;

namespace EECS395Library
{
    public static class Search
    {
        /// <summary>
        /// A search heuristic to estimate the distance between two nodes. If this heuristic always underestimates the cost, the resulting A* path will be optimal.
        /// </summary>
        /// <typeparam name="T">The type of nodes to compare.</typeparam>
        /// <param name="current">The current node.</param>
        /// <param name="goal">The goal node.</param>
        /// <returns>A real (double) number estimating the cost.</returns>
        public delegate double Heuristic<T>(T current, T goal) where T : INode<T>;

        /// <summary>
        /// A heuristic that estimates the cost to the goal based on the current node.
        /// </summary>
        public delegate double GoalHeurstic<T>(T current) where T : INode<T>;

        /// <summary>
        /// Tests if we are at the goal.
        /// </summary>
        public delegate bool GoalTest<T>(T current) where T : INode<T>;

        /// <summary>
        /// Implements A* search. Finds the path, if one exists, from the start node to the goal node.
        /// </summary>
        /// <typeparam name="T">The type of nodes in the graph.</typeparam>
        /// <param name="start">The starting node or initial state.</param>
        /// <param name="heuristic">The heuristic function to use.</param>
        /// <param name="goalTest">A goal test to use to determine when the search completes</param>
        /// <param name="maxDistance">The maximum path length to search for. Search will quit if the cost to no remaining nodes is less than the maximum length.</param>
        /// <param name="initialQueueSize">(OPTIONAL) The initial size of the priority queue. Does not affect the outcome of the search.</param>
        /// <param name="queueNumberOfChildren">(OPTIONAL) The number of children per parent in the heap-based priority queue. Does not affect the outcome of the search.</param>
        /// <returns>The path, if one was found, otherwise null.</returns>
        public static List<T> AStar<T>(T start, GoalTest<T> goalTest, GoalHeurstic<T> heuristic, double maxDistance, int initialQueueSize = DAryHeap.DefaultHeapSize, int queueNumberOfChildren = DAryHeap.DefaultNumberOfChildren) where T : class, INode<T>
        {
            Dictionary<T, NodeInfo<T>> nodeInformation = new Dictionary<T, NodeInfo<T>>();

            DAryHeap<T> queue = new DAryHeap<T>(initialQueueSize, queueNumberOfChildren);

            T currentNode = start;

            var currentNodeInfo = nodeInformation.GetNodeInfo(currentNode);

            currentNodeInfo.G = 0f;

            currentNodeInfo.F = heuristic(currentNode);

            // by definition, the initial G value is zero, so G + F = F
            queue.Insert(currentNode, currentNodeInfo.F);

            while (!goalTest(currentNode))
            {
                if (queue.Count == 0) return null;

                currentNode = queue.ExtractMin().Item; // pop the lowest cost node

                currentNodeInfo = nodeInformation.GetNodeInfo(currentNode);
                currentNodeInfo.Expanded = true;

                foreach (var edge in currentNode.Edges) // for each of its neighbors
                {
                    T neighbor = edge.Node;

                    var neighborInfo = nodeInformation.GetNodeInfo(neighbor);

                    // If we haven't expanded it (found the path to it)
                    if (!neighborInfo.Expanded)
                    {
                        double g = currentNodeInfo.G + edge.Weight;
                        double f = g + heuristic(neighbor);

                        // If it's not in the queue
                        if (neighborInfo.Predecessor == null)
                        {
                            // Only add the neighbor to the queue if the new path length would go over our max, don't add it to the queue
                            if (g <= maxDistance)
                            {
                                // Set its G and F values
                                neighborInfo.Predecessor = currentNode;
                                neighborInfo.G = g;
                                neighborInfo.F = f;

                                // Add it to the queue
                                queue.Insert(neighbor, f);
                            }
                        }
                        // If it is in the heap, but we've found a cheaper path to this noe
                        else if (f < neighborInfo.F)
                        {
                            neighborInfo.Predecessor = currentNode;
                            neighborInfo.G = g;
                            neighborInfo.F = f;
                            queue.Insert(neighbor, f);
                        }
                    }
                }
            }

            // trace back the path
            List<T> path = new List<T>();

            while (currentNode != null)
            {
                path.Add(currentNode);
                currentNode = GetNodeInfo(nodeInformation, currentNode).Predecessor;
            }

            path.Reverse();
            return path;
        }


        /// <summary>
        /// Implements A* search. Finds the path, if one exists, from the start node to the goal node.
        /// </summary>
        /// <typeparam name="T">The type of nodes in the graph.</typeparam>
        /// <param name="start">The starting node or initial state.</param>
        /// <param name="goal">The final node or goal state.</param>
        /// <param name="heuristic">The heuristic function to use.</param>
        /// <param name="maxDistance">The maximum path length to search for. Search will quit if the cost to no remaining nodes is less than the maximum length.</param>
        /// <param name="initialQueueSize">(OPTIONAL) The initial size of the priority queue. Does not affect the outcome of the search.</param>
        /// <param name="queueNumberOfChildren">(OPTIONAL) The number of children per parent in the heap-based priority queue. Does not affect the outcome of the search.</param>
        /// <returns>The path, if one was found, otherwise null.</returns>
        public static List<T> AStar<T>(T start, T goal, Heuristic<T> heuristic, double maxDistance, int initialQueueSize = DAryHeap.DefaultHeapSize, int queueNumberOfChildren = DAryHeap.DefaultNumberOfChildren) where T : class, INode<T>
        {
            Dictionary<T, NodeInfo<T>> nodeInformation = new Dictionary<T, NodeInfo<T>>();

            DAryHeap<T> queue = new DAryHeap<T>(initialQueueSize, queueNumberOfChildren);

            T currentNode = start;

            var currentNodeInfo = nodeInformation.GetNodeInfo(currentNode);

            currentNodeInfo.G = 0f;

            currentNodeInfo.F = heuristic(currentNode, goal);

            // by definition, the initial G value is zero, so G + F = F
            queue.Insert(currentNode, currentNodeInfo.F);

            while (currentNode != goal)
            {
                if (queue.Count == 0) return null;

                currentNode = queue.ExtractMin().Item; // pop the lowest cost node

                currentNodeInfo = nodeInformation.GetNodeInfo(currentNode);
                currentNodeInfo.Expanded = true;

                foreach (var edge in currentNode.Edges) // for each of its neighbors
                {
                    T neighbor = edge.Node;

                    var neighborInfo = nodeInformation.GetNodeInfo(neighbor);

                    // If we haven't expanded it (found the path to it)
                    if (!neighborInfo.Expanded)
                    {
                        double g = currentNodeInfo.G + edge.Weight;
                        double f = g + heuristic(neighbor, goal);

                        // If it's not in the queue
                        if (neighborInfo.Predecessor == null)
                        {
                            // Only add the neighbor to the queue if the new path length would go over our max, don't add it to the queue
                            if (g <= maxDistance)
                            {
                                // Set its G and F values
                                neighborInfo.Predecessor = currentNode;
                                neighborInfo.G = g;
                                neighborInfo.F = f;

                                // Add it to the queue
                                queue.Insert(neighbor, f);
                            }
                        }
                        // If it is in the heap, but we've found a cheaper path to this noe
                        else if (f < neighborInfo.F)
                        {
                            neighborInfo.Predecessor = currentNode;
                            neighborInfo.G = g;
                            neighborInfo.F = f;
                            queue.Insert(neighbor, f);
                        }
                    }
                }
            }

            // trace back the path
            List<T> path = new List<T>();

            while (currentNode != null)
            {
                path.Add(currentNode);
                currentNode = GetNodeInfo(nodeInformation, currentNode).Predecessor;
            }

            path.Reverse();
            return path;
        }

        /// <summary>
        /// Implements A* search. Finds the path, if one exists, from the start node to the goal node.
        /// </summary>
        /// <typeparam name="T">The type of nodes in the graph.</typeparam>
        /// <param name="start">The starting node or initial state.</param>
        /// <param name="goal">The final node or goal state.</param>
        /// <param name="heuristic">The heuristic function to use.</param>
        /// <param name="initialQueueSize">(OPTIONAL) The initial size of the priority queue. Does not affect the outcome of the search.</param>
        /// <param name="queueNumberOfChildren">(OPTIONAL) The number of children per parent in the heap-based priority queue. Does not affect the outcome of the search.</param>
        /// <returns>The path, if one was found, otherwise null.</returns>
        public static List<T> AStar<T>(T start, T goal, Heuristic<T> heuristic, int initialQueueSize = DAryHeap.DefaultHeapSize, int queueNumberOfChildren = DAryHeap.DefaultNumberOfChildren) where T : class, INode<T>
        {
            return AStar(start, goal, heuristic, double.PositiveInfinity, initialQueueSize, initialQueueSize);
        }

        /// <summary>
        /// Implements Djikstra's algorithm. Finds the shortest path, if one exists, from the start node to the goal node.
        /// </summary>
        /// <typeparam name="T">The type of nodes in the graph.</typeparam>
        /// <param name="start">The starting node or initial state.</param>
        /// <param name="goal">The final node or goal state.</param>
        /// <param name="initialQueueSize">(OPTIONAL) The initial size of the priority queue. Does not affect the outcome of the search.</param>
        /// <param name="queueNumberOfChildren">(OPTIONAL) The number of children per parent in the heap-based priority queue. Does not affect the outcome of the search.</param>
        /// <returns>The path, if one was found, otherwise null.</returns>
        public static List<T> Djikstra<T>(T start, T goal, int initialQueueSize = DAryHeap.DefaultHeapSize, int queueNumberOfChildren = DAryHeap.DefaultNumberOfChildren) where T : class, INode<T>
        {
            return AStar(start, goal, (current, target) => 0, initialQueueSize, queueNumberOfChildren);
        }

        class NodeInfo<T> where T : INode<T>
        {
            public double G { get; set; }
            public double F { get; set; }
            public T Predecessor { get; set; }
            public bool Expanded { get; set; }
        }

        // Helper method. If we try to access a node's information and we don't have an entry for it in the dictionary, it will create and initialize it for us.
        private static NodeInfo<T> GetNodeInfo<T>(this Dictionary<T, NodeInfo<T>> dictionary, T key) where T : INode<T>
        {
            NodeInfo<T> nodeInfo;
            dictionary.TryGetValue(key, out nodeInfo);

            if (nodeInfo == null)
            {
                nodeInfo = new NodeInfo<T>();
                dictionary[key] = nodeInfo;
            }

            return nodeInfo;
        }
    }
}